/* --------------------------------------------------------------------------
 * GraphomatJr DrawBot Example
 * --------------------------------------------------------------------------
 * prog:  Max Rheiner / Interaction Design / Zhdk / http://iad.zhdk.ch/
 * date:  08/26/2012 (m/d/y)
 * ----------------------------------------------------------------------------
 *
 * ----------------------------------------------------------------------------
 */

import processing.serial.*;

import javax.media.opengl.*;
import javax.media.opengl.glu.*;
import javax.media.opengl.GL2;

import peasy.*;
import SimpleKDL.*;
import SimpleDynamixel.*;
import geomerative.*;


final static int MODE_NORMAL	= 0;
final static int MODE_FREEHAND	= 1;

PeasyCam cam;

PVector endPoint = new PVector();
PVector endPointNormal = new PVector();
PVector endPointRotX = new PVector(1,0,0);
PVector endPointRotY = new PVector(0,1,0);
PVector endPointRotZ = new PVector(0,0,-1);
PMatrix3D endPointMat = new PMatrix3D();

int planeType = 0;
int mousePick = 0;

int mode = MODE_NORMAL;

// define picking plane x-y
PickHelpers pickHelpers = new PickHelpers();

PVector pickPlaneP1 = new PVector(0, 0, 0);
PVector pickPlaneP2 = new PVector(1, 0, 0);
PVector pickPlaneP3 = new PVector(0, 1, 0);
boolean lastPick = false;

// picking trail
ArrayList  pickTrail = new ArrayList();
int pickTrailMax = 900;

boolean		plotStepFlag = false;
PathReader  plotterOut;
RShape      svgDrawing = null;
RFont 		font;
PVector     svgDrawingTrans;

PVector		sceneTrans = new PVector(0,0,0);

RoboServoController		roboController;

void setup() 
{
  //hint(ENABLE_OPENGL_4X_SMOOTH);  // antialiasing is somehow buggy in processing ?!
  smooth(4);
//  size(1280, 720, OPENGL);
  size(1000, 600, OPENGL);


  // init geomertry library
  RG.init(this);

  // setup cam
  cam = new peasy.PeasyCam(this, 1000);
  cam.setMinimumDistance(.001);
  cam.setMaximumDistance(5000);

  ///////////////////////////////////////////////////////////
  // init the dynamixel library
  //String serialDev = "/dev/tty.usbserial-A7003MkL"; // osx
  String serialDev = "/dev/ttyUSB0"; // linux
  Servo servo = new Servo();
  servo.init(serialDev,1000000); 	// 1Mbit baudrate , use c++ serial
 
  ///////////////////////////////////////////////////////////
  // start SimpleKdl
  SimpleKDLContext.init(this);

  ///////////////////////////////////////////////////////////
  // setup for picking
  pickHelpers.init(this);
  
  ///////////////////////////////////////////////////////////
  // setup the controller for the robot
  roboController = new GraphomatJr(this,	
								   servo);

  ///////////////////////////////////////////////////////////
  // setup the drawing content
  svgDrawing = RG.loadShape("testVec1.svg");
  //svgDrawing = RG.loadShape("mario1.svg");

  //svgDrawing = RG.loadShape("maxMoritz2.svg");
  //svgDrawing = RG.loadShape("test2.svg");
  //svgDrawing = RG.loadShape("s.svg");
  svgDrawingTrans = new PVector(-svgDrawing.getWidth() / 2,-svgDrawing.getHeight() );
  
  font = new RFont("LiberationSans-Bold.ttf", 80, RFont.CENTER);
 
  // calculate the center for the svg drawing
  RPoint center = svgDrawing.getCenter();
  svgDrawingTrans.x = -center.x;
  svgDrawingTrans.y = -center.y;

  plotterOut = new PathReader();

  // capture the drawing 
  plotterOut.beginDraw();
	drawScene(plotterOut);
  plotterOut.endDraw();

  ///////////////////////////////////////////////////////////
  // feed the data to the roboter
  roboController.setPolygonList(plotterOut.lineList());
  roboController.setPriority(Thread.MAX_PRIORITY);
  roboController.start();

  // check if the calibration file is there and load it
  roboController.loadCalibrationPoints(dataPath("saveCalibration.cal"));
}

void drawScene(PGraphics g)
{  // 2d
  g.noFill();

  //////////////////////////////////////////////
  // draw svg
  g.pushMatrix();
	g.translate(sceneTrans.x,sceneTrans.y,sceneTrans.z);
	g.stroke(255,0,0);

	if(svgDrawing != null)
	{
	  g.pushMatrix();
		g.translate(svgDrawingTrans.x,svgDrawingTrans.y,svgDrawingTrans.z);

		svgDrawing.setStroke(true);
		svgDrawing.draw(g);
	  g.popMatrix();
	}
	
	g.pushMatrix();
	  // mirror font
	  //g.scale(-1,1,1);

	 // font.draw("e",g);
	  font.draw("HELLO",g);
	g.popMatrix();


   // g.ellipse(0,0, 50, 50);
/*
	// not supported at the moment
    g.bezier(85, 20, 10, 10, 90, 90, 15, 80);

	g.pushMatrix();
	
 	//g.rotate(radians(90));
    g.ellipse(-40,60, 55, 55);
	g.popMatrix();

//   line(-200.0f,-100.0f,
//			   100.0f,-200.0f);

    g.point(11,22);  
*/
  g.popMatrix();
}

void mousePressed()
{
  if((mouseButton == LEFT || mouseButton == RIGHT) && 
	 (keyPressed == true) && (key == CODED) && (keyCode == SHIFT))
  {
    noCursor();
    mousePick = 1;
    
    // switch off the left dragging of camera
    cam.setActive(false);
    
    pickScene(mouseButton);
  }
}

void mouseDragged()
{
  if(mousePick == 0)
    return;  

  pickScene(mouseButton); 
}

void mouseReleased()
{
  if(mousePick == 1)
  {
    cursor();
    mousePick = 0;
    
    // switch on the left dragging of camera
    cam.setActive(true);
  }    
}

void draw() 
{
  // set the lights
  ambientLight(100, 100,100);
  directionalLight(200, 172, 235,
                   1, -1, 0);

  background(0);
/* // viz robo drawing scene
  pushMatrix();
	drawScene(g);
  popMatrix();
*/
  //////////////////////////////////////////////
  // draw picking plane / floor
  pushStyle();
  strokeWeight(1);
  stroke(150, 150, 150, 100);
  Utils.drawPlane(g,pickPlaneP1, pickPlaneP2, pickPlaneP3, 
                  2000, 21);
  popStyle();  

  //////////////////////////////////////////////
  // draw pickTrail
  if(pickTrail.size() > 0)
  {
    PVector vec;
    float q = 1.0f / pickTrail.size();
    noFill();
    beginShape();
    for(int i=0;i < pickTrail.size();i++)
    {
      vec = (PVector) pickTrail.get(i);
      stroke(19, 200, 170, 255 * q * i);
      vertex(vec.x,vec.y,vec.z);
    }
    endShape();
  }
  
  //////////////////////////////////////////////
  // draw picking pos
  pushStyle();
  pushMatrix();
  applyMatrix(endPointMat);
  strokeWeight(3);  
  Utils.drawCoordSys(g,50);
  strokeWeight(1);  
  popMatrix();
  
  pushMatrix();
  translate(endPoint.x, endPoint.y, endPoint.z);
  
  if(lastPick)
  {
    fill(0, 255, 0,100);
    stroke(0, 255, 0,100);
  }    
  else 
  {
    fill(255, 0, 0,100);
    stroke(255, 0, 0,100);
  }  

  line(0,0,0,
       endPointNormal.x * 100,endPointNormal.y  * 100,endPointNormal.z  * 100);
  
  noStroke();
  sphere(10);
  popMatrix();

  /////////////////////////////////////////////////////////////////////////////
  //  draw robot
  roboController.draw(g);

  // draw the head up display
  cam.beginHUD();
	noLights();
	roboController.draw2d(g);
  cam.endHUD();
  
}

void keyPressed()
{
  switch(key)
  {
  // plane calibration
  case 'a':
	roboController.startPlaneScanner();
	break;
  case 's':
	roboController.capturePlaneScanner();
	break;
  case 'd':
	if(roboController.endPlaneScanner())
	{
	  PVector center = new PVector();
	  PVector xPos = new PVector();
	  PVector yPos = new PVector();

	  roboController.getCalibratePoints(center,xPos,yPos);

	  println("calbiration points:");
	  println("center" + center);
	  println("xPos" + xPos);
	  println("yPos" + yPos);

	  roboController.calibrate(center,xPos,yPos);

	  // save position
	  roboController.saveCalibrationPoints(dataPath("saveCalibration.cal"));
	}
	break;

  // record
  case 'f':
	roboController.startPose();
	break;

  // visual props
  case '1':
	roboController.setVizFlags(roboController.vizFlags() ^ RoboServoController.ROBO_VIZ_INFO);
	break;
  case ' ':
	if(roboController.isDrawing())
	  // robo is drawing, pause
	  roboController.pauseDrawing();
	else
	{
	  if(roboController.state() == RoboServoController.ROBO_STATE_PAUSE)
		// robo is paused, play
		roboController.playDrawing();
	  else
		// start to draw
		roboController.startDrawing();
	}
	break;
  case 'e':
	roboController.setEnable(!roboController.enable());
	break;
  case 'p':
    float[] angles = roboController.kinematicSolver().getOutAngles();

    println("--------------------------");
    println("Current angles in degrees:");
    for(int i=0; i < angles.length;i++)
      println("joint" + i + " = " +  degrees(angles[i]));
      
    println("Current position:" + endPoint);
          
    break;
  // reset the drawing, go to start
  case 'x':
	roboController.stopDrawing();
    break;    
  case 'k':
	if(mode == MODE_FREEHAND)
	  mode = MODE_NORMAL;
	else if(mode == MODE_NORMAL)
	{
	  mode = MODE_FREEHAND;
	  roboController.setMovingSpeed(0);
	}
	break;
  }
}

// callback from the robots, indicates in which state it is
void onRoboStatus(RoboServoController roboServoController,int status)
{
  switch(status)
  {
  case RoboServoController.ROBO_STATE_START:
	//println("Robot start drawing");
	break;
  case RoboServoController.ROBO_STATE_END:
	//println("Robot end drawing");
	break;
  case RoboServoController.ROBO_STATE_DRAWING_START:
	//println("Robot pen down start drawing");
	break;
  case RoboServoController.ROBO_STATE_DRAWING_END:
	//println("Robot pen up end drawing");
	break;
  }
  //println("onRoboStatus: " + status);
}


// let's do some handdrawing with the robot
void pickScene(int mouseButton)
{
  PVector r1 = new PVector();
  PVector r2 = new PVector();
  
  // get the current hit ray
  pickHelpers.getHitRay(mouseX,mouseY,
                        r1,r2);
                    
  PVector rDir = PVector.sub(r2,r1);
  PVector pick3d = new PVector();
  PVector pickNormal = new PVector();
  
  PVector p = new PVector();
  PVector pX = new PVector();
  PVector pY = new PVector();

  // get the current drawing plane
  roboController.getPlane(p,pX,pY);

  // check if the hit was on the plane               
  if(RayIntersection.plane(r1, rDir, 
                           p, pX, pY, 
                           pick3d))
  {
    pickNormal = Utils.getPlaneNormal(p, pX, pY);
	
	if(mode == MODE_FREEHAND)
	{
	  if(mouseButton == RIGHT)
	  {	// draw
		lastPick = roboController.setEndPosGlobal(pick3d,pickNormal);
		if(lastPick)
		{
		  roboController.setCurPosition();

		  // add to pickTrail
		  pickTrail.add(pick3d.get());
		  if(pickTrail.size() > pickTrailMax)
			// remove the oldest
			pickTrail.remove(0);
		}
	  }
	  else if(mouseButton == LEFT)
	  {	// move to robo to the pre draw pos
		pick3d = PVector.add(pick3d,PVector.mult(pickNormal,20));	// 20mm above the ground
		lastPick = roboController.setEndPosGlobal(pick3d,pickNormal);
		if(lastPick)
		{		
		  roboController.setCurPosition();
		}
	  }	  
	}
  }
}
 
